package com.AX.service.impl;
import com.AX.entity.*;
import com.AX.exception.NoPathException;
import com.AX.mapper.DynamicObstacleMapper;
import com.AX.mapper.NavEdgeMapper;
import com.AX.mapper.NavNodeMapper;
import com.AX.service.PathService;
import com.AX.utils.GeoUtils;
import jakarta.annotation.PostConstruct;
import lombok.RequiredArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.scheduling.annotation.Scheduled;
import org.springframework.stereotype.Service;

import java.util.*;
import java.util.concurrent.ConcurrentHashMap;
import java.util.concurrent.CopyOnWriteArraySet;
import java.util.concurrent.TimeUnit;
import java.util.stream.Collectors;


@Slf4j
@Service
@RequiredArgsConstructor
public class AStarPathServiceImpl implements PathService {

    // 数据访问组件
    private final NavNodeMapper navNodeMapper;
    private final NavEdgeMapper navEdgeMapper;
    private final DynamicObstacleMapper dynamicObstacleMapper;

    // 图数据结构（线程安全）
    private volatile Map<String, NavNode> nodeMap = new ConcurrentHashMap<>();
    private volatile Map<String, List<NavEdge>> edgeMap = new ConcurrentHashMap<>();

    // 动态障碍数据（线程安全）
    private volatile Set<String> blockedNodes = new CopyOnWriteArraySet<>();
    private volatile Map<Integer, Double> edgeAdjustments = new ConcurrentHashMap<>();


    /**
     * 加载静态图数据（停车场结构）
     */
    private void loadStaticData(Integer parkingLot) {
        log.info("开始加载静态图数据...");
        long startTime = System.currentTimeMillis();

        // 1. 加载节点数据（停车场ID=1）
        List<NavNode> nodes = navNodeMapper.selectByParkingLot(parkingLot);
        nodeMap = nodes.parallelStream()
                .collect(Collectors.toConcurrentMap(
                        NavNode::getNodeId,
                        node -> node,
                        (oldNode, newNode) -> oldNode
                ));

        // 2. 构建邻接表（节点ID -> 连接边列表）
        edgeMap = nodes.parallelStream()
                .collect(Collectors.toConcurrentMap(
                        NavNode::getNodeId,
                        node -> Collections.unmodifiableList(
                                navEdgeMapper.findConnectedEdges(node.getNodeId())
                        )
                ));

        log.info("静态图数据加载完成，节点数：{}，耗时：{}ms",
                nodes.size(), System.currentTimeMillis() - startTime);
    }

    /**
     * 定时刷新动态障碍（每分钟执行）
     */
    @Scheduled(fixedRate = 60, timeUnit = TimeUnit.SECONDS)
    public void refreshDynamicObstacles(Integer parkingLotId) {
        log.debug("开始刷新动态障碍数据...");
        long startTime = System.currentTimeMillis();

        // 1. 获取当前被封锁的节点
        Set<String> newBlockedNodes = new CopyOnWriteArraySet<>(
                dynamicObstacleMapper.findActiveBlockedNodes(parkingLotId)
        );

        // 2. 获取动态边调整系数（key=边ID，value=调整系数）
        Map<Integer, Double> newEdgeAdjustments = new ConcurrentHashMap<>(
                dynamicObstacleMapper.findActiveEdgeAdjustments(parkingLotId)
        );

        // 3. 原子化更新引用
        blockedNodes = newBlockedNodes;
        edgeAdjustments = newEdgeAdjustments;

        log.debug("动态障碍刷新完成，影响节点：{}个，影响边：{}条，耗时：{}ms",
                newBlockedNodes.size(), newEdgeAdjustments.size(),
                System.currentTimeMillis() - startTime);
    }

    /**
     * 核心路径计算方法
     */
    @Override
    public PathResponse calculatePath(PathRequest request) throws NoPathException {

        loadStaticData(request.getParkingLotId());
        refreshDynamicObstacles(request.getParkingLotId());
        // 参数校验
        //validateRequest(request);

        // 1. 查找起点节点
        NavNode startNode = findStartNode(request);

        // 2. 查找目标节点
        NavNode targetNode = findTargetNode(request);

        // 3. 执行A*算法（含动态障碍处理）
        List<String> path = findPathWithObstacles(
                startNode.getNodeId(),
                targetNode.getNodeId()
        );

        // 4. 生成导航响应
        return buildResponse(path, request.getCurrentHeading());
    }

    /**
     * 增强版A*算法实现（含障碍处理）
     */
    private List<String> findPathWithObstacles(String startId, String endId)
            throws NoPathException {
        final NavNode targetNode = nodeMap.get(endId);
        if (targetNode == null) {
            throw new NoPathException("目标节点不存在");
        }

        PriorityQueue<NodeWrapper> openSet = new PriorityQueue<>();
        Map<String, NodeWrapper> allNodes = new ConcurrentHashMap<>();

        // 初始化起点
        NodeWrapper start = new NodeWrapper(
                nodeMap.get(startId),
                0.0,
                targetNode
        );
        openSet.add(start);
        allNodes.put(startId, start);

        while (!openSet.isEmpty()) {
            NodeWrapper current = openSet.poll();

            // 到达终点
            if (current.node.getNodeId().equals(endId)) {
                return reconstructPath(current);
            }

            // 处理所有相邻边
            edgeMap.getOrDefault(current.node.getNodeId(), Collections.emptyList())
                    .forEach(edge -> processEdge(current, edge, allNodes, openSet));
        }

        throw new NoPathException("未找到可行路径，可能原因：路径被封锁或障碍物阻断");
    }

    /**
     * 处理单条路径边（含障碍检测）
     */
    private void processEdge(NodeWrapper current, NavEdge edge,
                             Map<String, NodeWrapper> allNodes,
                             PriorityQueue<NodeWrapper> openSet) {
        // 1. 检查边是否被完全阻断
        Double adjustment = edgeAdjustments.get(edge.getEdgeId());
        if (adjustment != null && adjustment < 0) {
            log.trace("边 {} 被完全阻断，跳过处理", edge.getEdgeId());
            return;
        }

        // 2. 确定邻居节点方向
        boolean isForward = edge.getStartNode().equals(current.node.getNodeId());
        String neighborId = isForward ? edge.getEndNode() : edge.getStartNode();

        // 3. 检查节点是否被封锁
        if (blockedNodes.contains(neighborId)) {
            log.trace("节点 {} 被封锁，跳过处理", neighborId);
            return;
        }

        // 4. 获取节点对象
        NavNode neighborNode = nodeMap.get(neighborId);
        if (neighborNode == null) return;

        // 5. 计算实际移动成本
        double baseCost = edge.getWeight();
        double actualCost = (adjustment != null && adjustment > 0) ?
                baseCost * adjustment : // 应用调整系数
                baseCost;

        // 6. 创建/获取节点包装器
        NodeWrapper neighbor = allNodes.computeIfAbsent(neighborId, id ->
                new NodeWrapper(neighborNode, Double.MAX_VALUE, current.target)
        );

        // 7. 计算临时成本
        double tentativeG = current.g + actualCost;
        if (tentativeG < neighbor.g) {
            neighbor.parent = current;
            neighbor.g = tentativeG;
            neighbor.f = neighbor.g + neighbor.h;

            // 更新优先队列
            if (!openSet.contains(neighbor)) {
                openSet.add(neighbor);
            }
        }
    }

    /**
     * 路径重构方法
     */
    private List<String> reconstructPath(NodeWrapper endNode) {
        LinkedList<String> path = new LinkedList<>();
        NodeWrapper current = endNode;
        while (current != null) {
            path.addFirst(current.node.getNodeId());
            current = current.parent;
        }
        return path;
    }

    private void validateRequest(PathRequest request) {
        if (request.getCurrentPosition() == null) {
            throw new IllegalArgumentException("当前位置不能为空");
        }
        if (request.getTargetSpaceId() == null || request.getTargetSpaceId().isEmpty()) {
            throw new IllegalArgumentException("目标车位ID不能为空");
        }
    }

    private NavNode findStartNode(PathRequest request) throws NoPathException {
        NavNode node = navNodeMapper.findNearestNode(
                request.getCurrentPosition().getLng(),
                request.getCurrentPosition().getLat(),
                request.getParkingLotId() // 停车场ID
        );
        if (node == null) {
            throw new NoPathException("未找到附近的路径起点");
        }
        return node;
    }

    private NavNode findTargetNode(PathRequest request) throws NoPathException {
        NavNode node = navNodeMapper.findBySpaceId(request.getTargetSpaceId());
        if (node == null) {
            throw new NoPathException("目标车位不存在或未关联路径节点");
        }
        return node;
    }

    private PathResponse buildResponse(List<String> path, Double initialHeading) {
        List<PathStep> steps = new ArrayList<>();
        double totalDistance = 0;
        Double currentHeading = initialHeading;

        for (int i = 0; i < path.size() - 1; i++) {
            NavNode current = nodeMap.get(path.get(i));
            NavNode next = nodeMap.get(path.get(i + 1));

            // 计算导航步骤
            double distance = GeoUtils.haversineDistance(
                    current.getPosition(),
                    next.getPosition()
            );
            double bearing = GeoUtils.calculateBearing(
                    current.getPosition(),
                    next.getPosition()
            );

            steps.add(new PathStep(
                    calculateTurnAction(currentHeading, bearing),
                    String.format("%.1f米", distance),
                    next.getNodeId()
            ));

            totalDistance += distance;
            currentHeading = bearing;
        }

        return new PathResponse(
                path,
                steps,
                totalDistance,
                (int) (totalDistance / 1.4) // 假设平均速度1.4m/s
        );
    }

    private String calculateTurnAction(Double fromHeading, Double toHeading) {
        if (fromHeading == null) return "直行";
        double diff = (toHeading - fromHeading + 360) % 360;
        if (diff < 22.5 || diff > 337.5) return "直行";
        if (diff < 67.5) return "右前方转弯";
        if (diff < 112.5) return "右转";
        if (diff < 157.5) return "右后方转弯";
        if (diff < 202.5) return "调头";
        if (diff < 247.5) return "左后方转弯";
        if (diff < 292.5) return "左转";
        return "左前方转弯";
    }

    // ---------------------- 内部类 ----------------------
    private static class NodeWrapper implements Comparable<NodeWrapper> {
        final NavNode node;
        final NavNode target;
        double g;  // 实际成本
        final double h;  // 启发式成本
        double f;  // 总成本 (g + h)
        NodeWrapper parent;

        NodeWrapper(NavNode node, double g, NavNode target) {
            this.node = node;
            this.g = g;
            this.target = target;
            this.h = GeoUtils.haversineDistance(
                    node.getPosition(),
                    target.getPosition()
            );
            this.f = this.g + this.h;
        }

        @Override
        public int compareTo(NodeWrapper other) {
            return Double.compare(this.f, other.f);
        }
    }
}
